#include <Wire.h>
#include <NewSoftSerial.h>
#include <TinyGPS.h>

// Config GPS
TinyGPS gps; // Create an instance of the TinyGPS object
NewSoftSerial uart_gps(2, 3); // PIN GPS

// Config boussole
int HMC6352SlaveAddress = 0x42; 
int HMC6352ReadAddress = 0x41;
boolean depart_GPS=false;

// Config TP20
int butee_safran=5;
int position_safran=0;
boolean virement_de_bord=false;
//char Fleche_D[24]="3F 24 3F 34 3F 20 3F 30"; // Trames TP20
//char Fleche_G[24]="3F 28 3F 38 3F 20 3F 30";

char Fleche_D[24]="Droite"; // TESTS
char Fleche_G[24]="Gauche";

// Config WP
long R_WP=15; // Rayon WP
long R_GPS; // Rayon comparaison GPS / WP
float WP_lat[3]; // WP___[0]=WPDepart et WP___[1]=WPPosition actuelle et WP___[2]=WPObjectif actuel
float WP_long[3]; 
float Objectifs_WP_lat[]={
  48.27653845094154,0}; // Objectifs WP latitude
float Objectifs_WP_long[]={
  -4.412807822227478,0}; // Objectifs WP longitude
int compteurWP=0;
boolean cap_optimise=false;
boolean depart=false;
boolean depart_optimisation=false;

// Const
const float pi=3.141592654;
const long rmoyterre=6371030;

void setup(){
  // Config boussole
  HMC6352SlaveAddress = HMC6352SlaveAddress >> 1;

  // Config TP20
  int txPin = 1;                    // TX
  pinMode(txPin, OUTPUT);

  uart_gps.begin(38400); // Baud avec GPS
  Serial.begin(115200); // Baud necessaire pour affichage correct des donnees GPS
  Wire.begin();
}

void Boussole(float* heading){
  Wire.beginTransmission(HMC6352SlaveAddress);
  Wire.send(HMC6352ReadAddress);              
  Wire.endTransmission();
  delay(6); // Time delays required by HMC6352
  Wire.requestFrom(HMC6352SlaveAddress, 2); // Get the two data bytes, MSB and LSB
  byte MSB = Wire.receive();
  byte LSB = Wire.receive();
  float headingSum = (MSB << 8) + LSB; 
  float headingInt = headingSum / 10; 
  headingInt = radians(headingInt); // Passage cap en rad
  heading[0]= cos(headingInt);
  heading[1]= sin(headingInt);
}

void BoussoleLissee(float* caplisse){ // Moy de n caps prélevés
  int nbrepointslissage=15; 
  float cap[2];
  float sinus[nbrepointslissage]; // Nbre pts lissage
  float cosinus[nbrepointslissage]; // Nbre pts lissage
  float sommecos=0;
  float sommesin=0;
  for (int i=0; i<nbrepointslissage; i++){ // Boucle enregistre les caps
    Boussole(cap);
    cosinus[i]=cap[0];
    sinus[i]=cap[1];
  }
  for (int i=0; i<nbrepointslissage; i++){ // Somme de tous les caps prélevés ci-dessus
    sommecos=sommecos+cosinus[i];
    sommesin=sommesin+sinus[i];
  }
  caplisse[0] = sommecos/nbrepointslissage;
  caplisse[1] = sommesin/nbrepointslissage;
}

void status_GPS(){
  int c = uart_gps.read();    // load the data into a variable...
  if(gps.encode(c))      // if there is a new valid sentence...
  {
    depart_GPS=true;
  }
  else{
    depart_GPS=false;
  }
}

long dist(float latitudedep, float longitudedep, float latitudear, float longitudear){ // Retourne la distance en mètres entre 2 points GPS // Utiliser rad !!
  float latitude;
  float longitude;
  float term1;
  float term2;
  float term3;
  float term4;
  long distance;
  latitude= absolue(latitudedep-latitudear);
  longitude= min(min(absolue(longitudedep-longitudear), absolue((2*pi+longitudedep)-longitudear)), absolue(longitudedep-(longitudear+2*pi)));
  term1 = (sq(sin(latitude/2.0))) + cos(latitudedep)*cos(latitudear)*(sq(sin(longitude/2.0)));
  term3=sqrt(term1);
  term4=sqrt(1-term1);
  term2 = 2*atan2(term3, term4);
  distance= rmoyterre*term2;
  return distance;
}

void GPSHeadingTh(float latitudedep, float longitudedep, float latitudear, float longitudear, float* cap){ // Retourne le cos le sin du cap entre 2 points GPS
  float heading;
  float term1;
  float term2;
  float term3;
  float term4;
  float longitude;
  longitude= min(min(absolue(longitudedep-longitudear), absolue((2*pi+longitudedep)-longitudear)), absolue(longitudedep-(longitudear+2*pi)));
  term1=sin(longitude)*cos(latitudear);
  term3=cos(latitudedep)*sin(latitudear);
  term4=sin(latitudedep)*cos(latitudear)*cos(longitude);
  term2=term3-term4;
  heading=atan2(term1,term2);
  if (sin(longitudear-longitudedep)<0){                    //passage de [-180°; 180°] à [0°; 360°] pour rejoindre la convention des caps d'un compas
    heading=2*pi-absolue(heading); 
  }
  cap[0]=cos(heading);
  cap[1]=sin(heading);
}

float ecartroute(float latitudedep, float longitudedep, float latitudear, float longitudear){ // Retourne l'écart à la route définie par les 2 points GPS en m
  float headingroute[2];
  GPSHeadingTh(latitudedep, longitudedep, latitudear, longitudear, headingroute);
  float proj[2];
  proj[0]=-headingroute[1];
  proj[1]=headingroute[0];
  float headingtocurrentpoint[2];
  GPSHeadingTh(latitudedep, longitudedep, radians(WP_lat[1]), radians(WP_long[1]), headingtocurrentpoint); 
  float distance_parcourue=dist(latitudedep, longitudedep, radians(WP_lat[1]), radians(WP_long[1]));        // Distance parcourue
  float ecart;
  ecart=distance_parcourue*(proj[0]*headingtocurrentpoint[0]+proj[1]*headingtocurrentpoint[1]); // sin(a-b)=... en partant de formule sinus d'un triangle 
  return ecart;
}

float absolue(float x){ // Oblige utiliser, car fct abs() bug avec <TinyGPS.h>
  if (x<0){
    return -x;
  }
  else{
    return x;
  }
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void coordonnees_route(){
  if (depart==false){
    delay(3000); // Pour GPS
    get_gps_position(gps,0); // Recup latitude et longitude GPS pour actualiser depart sur position actuelle

    WP_lat[2]=Objectifs_WP_lat[compteurWP]; // Actualise objectif
    WP_long[2]=Objectifs_WP_long[compteurWP]; 

    depart=true;
    depart_optimisation=false;
    Serial.println("New depart");   
  }

  get_gps_position(gps,1); // Recup latitude et longitude GPS pour actualiser position actuelle

  R_GPS=dist(radians(WP_lat[1]),radians(WP_long[1]),radians(WP_lat[2]),radians(WP_long[2])); // Distance du WPObjectif actuel
  Serial.print("Distance WP ");
  Serial.println(R_GPS);

  if (R_GPS<=R_WP && cap_optimise==false){ // WP atteint
    compteurWP++;
    get_gps_position(gps,0); // Recup latitude et longitude GPS pour actualiser depart sur position actuelle

    WP_lat[2]=Objectifs_WP_lat[compteurWP]; // Actualise objectif
    WP_long[2]=Objectifs_WP_long[compteurWP];
    Serial.println("WP atteind");
  }
  if (R_GPS<=R_WP && cap_optimise==true){ // WP optimise atteint, WPobjectif == celui de base 
    get_gps_position(gps,0); // Recup latitude et longitude GPS pour actualiser depart sur position actuelle

    WP_lat[2]=Objectifs_WP_lat[compteurWP]; // Actualise objectif
    WP_long[2]=Objectifs_WP_long[compteurWP];
    cap_optimise=false; 
    Serial.println("WP bis atteind");
  }
}

void get_gps_position(TinyGPS &gps, int nbr){ // Recup lat et long
  gps.f_get_position(&WP_lat[nbr], &WP_long[nbr]);
}

float Recup_cap_sinus_180(float* Vecteur_cap){ 
  float theta;
  if (Vecteur_cap[0]>0 && Vecteur_cap[1]>0){ // cos et sin > 0
    theta=asin(Vecteur_cap[1]);
  }
  else if (Vecteur_cap[0]<0 && Vecteur_cap[1]>0){ // cos < 0 et sin > 0
    theta=pi-asin(Vecteur_cap[1]);
  }
  else if (Vecteur_cap[0]<0 && Vecteur_cap[1]<0){ // cos < 0 et sin < 0
    theta=-pi-asin(Vecteur_cap[1]);
  }
  else{ // cos > 0 et sin < 0
    theta=asin(Vecteur_cap[1]);
  }
  return theta;  
}

void distance_1_pts_1_cap(float lat1, float long1, float theta, float d, float* lat2long2){ // Calcul lat2 et long2 avec lat1, long1, cap et distance 1 a 2
  lat2long2[0]=asin(sin(lat1)*cos(d/rmoyterre)+cos(lat1)*sin(d/rmoyterre)*cos(theta)); // Latitude
  lat2long2[1]=long1+atan2(sin(theta)*sin(d/rmoyterre)*cos(lat1),cos(d/rmoyterre)-sin(lat1)*sin(lat2long2[0])); // Longitude
}

void safran(int ordre){
  int correction=absolue(position_safran-ordre);

  if (ordre<position_safran){
    for (int i=0;i<correction;i++){
      if (position_safran > -butee_safran){
        //Serial.write(Fleche_D);
        position_safran--;
        delay(100);
      }
    }
  }
  else{
    for (int i=0;i<correction;i++){
      if (position_safran < butee_safran){
        //Serial.write(Fleche_G);
        position_safran++;
        delay(100);
      }
    }
  }
}

int SuiviCap(float latitudear, float longitudear){
  float theoricalheading[2];
  GPSHeadingTh(radians(WP_lat[1]), radians(WP_long[1]), latitudear, longitudear, theoricalheading); // PK ne pas utiliser position actuelle 1 ??
  float currentheading[2];
  float capboussole[2];
  float erreur;
  BoussoleLissee(capboussole);
  if (capboussole[0]*theoricalheading[0]+capboussole[1]*theoricalheading[1]>0){ // Dans le sens de la marche OK
    erreur=capboussole[0]*theoricalheading[1]-capboussole[1]*theoricalheading[0];    // Determinant donnant une image de l'erreur de cap, varie de -1 à 1 dans le sens de marche
    if (erreur>0){            // A TESTER valeurs erreurs !!!!!
      if (absolue(erreur)<0.3){
        return 0;
      }
      else if(absolue(erreur)<0.55){
        return -1;
      }
      else if(absolue(erreur)<0.75){
        return -2;
      }
      else{
        return -3;
      }
    }
    else{
      if (absolue(erreur)<0.3){
        return 0;
      }
      else if(absolue(erreur)<0.55){
        return 1;
      }
      else if(absolue(erreur)<0.75){
        return 2;
      }
      else{
        return 3;
      }
    }
  }
  else{     // Si on se trouve à l'envers du sens de marche
    if(capboussole[0]*theoricalheading[1]-capboussole[1]*theoricalheading[0]>0){
      // Virer sur tribord
      virement_de_bord=true;
      Serial.println("Virer sur tribord "); // TEST
      return -butee_safran; // A TESTER
    }
    else{
      // Virer sur babord
      virement_de_bord=true;
      Serial.println("Virer sur babord "); // TEST
      return butee_safran;  // A TESTER
    }
  }
}

int SuiviRoute(float latitudedep, float longitudedep, float latitudear, float longitudear){
  float ecart;
  // Calcul des variables nécessaires pour la fonction
  long longroute=dist(latitudedep, longitudedep, latitudear, longitudear);
  long largroute=longroute/5;
  //Calcul de la largeur de la route
  if (largroute>50000){
    largroute=50000;
  }
  if (largroute<30){
    largroute=30;
  } 
  ecart=ecartroute(latitudedep, longitudedep, latitudear, longitudear);  // Ecart à la route en mètres
  ecart=(2*ecart)/largroute;   // Ecart en %
  if (absolue(ecart)>1){ // Si hors route
    return butee_safran;
  }
  if (ecart<0){            // A TESTER valeurs erreurs !!!!!
    if (absolue(ecart)<0.3){
      return 0;
    }
    else if(absolue(ecart)<0.55){
      return -1;
    }
    else if(absolue(ecart)<0.75){
      return -2;
    }
    else{
      return -3;
    }
  }
  else{
    if (absolue(ecart)<0.3){
      return 0;
    }
    else if(absolue(ecart)<0.55){
      return 1;
    }
    else if(absolue(ecart)<0.75){
      return 2;
    }
    else{
      return 3;
    }
  }
}

boolean Correction_Route(float latitudear, float longitudear){
  int correction; 
  int Route;

  // Correction cap
  correction=SuiviCap(latitudear,longitudear);         // Ordre de correction cap

  // Correction ecart route
  if (virement_de_bord==false && depart_optimisation==true){ // Corrige ecart route si on est dans le sens de la route et si cap/vitesse optimise
    Route=SuiviRoute(radians(WP_lat[0]),radians(WP_long[0]), latitudear,longitudear);
    //Serial.println(Route);
    if (Route==butee_safran){ // Hors de la route
      depart=false; // Nouveau depart
      return false; // Fin de la fonction         
    }
    correction=correction+Route; // Corrige ecart a la route
  }

  safran(correction);

  if (position_safran<0){
    Serial.print("Droite "); // TEST
  }
  if (position_safran>0){
    Serial.print("Gauche "); // TEST
  }
  Serial.println(position_safran); // TEST
  delay(500);
}

////////////////////////////////////////////// Optimisation Cap/Vitesse //////////////////////////////////////////////

boolean Optimisation_cap_vitesse(){

  float Vitesse=gps.f_speed_kmph(); // Verification vitesse
  if (Vitesse > 15){
    return false;
  }

  // Calcul vitesse sur cap demande
  int nbrVitesse=5;
  Vitesse=0;
  float Vitesses[nbrVitesse];

  // Calcul vitesse pour cap demande
  Serial.println("Calcul vitesse pour cap demande"); // TEST
  for (int i=0;i<nbrVitesse;i++){
    Correction_Route(radians(WP_lat[2]),radians(WP_long[2])); // Corrige cap demande 
    Vitesses[i]=gps.f_speed_kmph(); // Fonction recup vitesse
    if (Vitesses[i] > 15){
      Vitesses[i]=0;
    }
    Serial.println(Vitesses[i]);
    delay(2000);
  }
  for (int i=0;i<nbrVitesse;i++){
    Vitesse=Vitesse+Vitesses[i];
  }
  Vitesse=Vitesse/nbrVitesse; // Vitesse moy
  Serial.println("Vitesse"); // TEST
  Serial.println(Vitesse); // TEST
  delay(1000); // TEST

  if (Vitesse > 1){ // Si on avance, on garde cette route
    Serial.println("Route sur cap demande");
    delay(1000); // TEST
    cap_optimise=false;
    return true;
  }

  // Sinon, on change de route
  get_gps_position(gps,0); // Recup latitude et longitude GPS pour actualiser depart
  get_gps_position(gps,1); // Recup latitude et longitude GPS pour actualiser position actuelle

  // Calcul des distance AB, AD et le cap demande
  float theoricalheading[2]; // Cap demande
  GPSHeadingTh(radians(WP_lat[0]), radians(WP_long[0]), radians(WP_lat[2]), radians(WP_long[2]), theoricalheading);
  float theta=Recup_cap_sinus_180(theoricalheading); // Cap demande
  long AB=dist(radians(WP_lat[0]), radians(WP_long[0]), radians(WP_lat[2]), radians(WP_long[2])); // Distance AB
  float AD_AG=AB/(2*cos(pi/4)); // Longueur tire des bords à 45 deg
  if (AD_AG > 2000){ // Max 2 km
    AD_AG=2000;
  }
  // Calcul des coordonnées du pts D
  float latDlongD[2]; // Coordonnées pts droite de la route 
  // Pts D
  theta=theta+(pi/4); 
  if (theta > pi){ // Passage -pi/pi
    theta=theta-2*pi;
  }
  distance_1_pts_1_cap(radians(WP_lat[0]),radians(WP_long[0]), theta, AD_AG, latDlongD); // Recup coordonnees

  // Calcul vitesse pour cap +45 deg
  Vitesse=0;
  Serial.println("Calcul vitesse pour cap +45"); // TEST
  for (int i=0;i<nbrVitesse;i++){ // Mise a 0
    Vitesses[i]=0;
  }
  Correction_Route(latDlongD[0],latDlongD[1]); // Corrige cap +45 deg // Attention, deja en rad !!
  for (int i=0;i<nbrVitesse;i++){
    Correction_Route(latDlongD[0],latDlongD[1]); // Corrige cap +45 deg 
    Vitesses[i]=gps.f_speed_kmph(); // Fonction recup vitesse
    if (Vitesses[i] > 15){
      Vitesses[i]=0;
    }
    Serial.println(Vitesses[i]);
    delay(2000);
  }
  for (int i=0;i<nbrVitesse;i++){
    Vitesse=Vitesse+Vitesses[i];
  }
  Vitesse=Vitesse/nbrVitesse; // Vitesse2 moy
  Vitesse=Vitesse*cos(pi/4); // VMG
  Serial.println("Vitesse +45"); // TEST
  Serial.println(Vitesse); // TEST
  delay(1000); // TEST

  if (Vitesse > 1){ // Si on avance, on garde cette route
    Serial.println("Route sur cap +45");
    delay(1000); // TEST
    WP_lat[2]=degrees(latDlongD[0]);
    WP_long[2]=degrees(latDlongD[1]);
    cap_optimise=true;
    return true;
  }

  // Sinon, on change de route
  get_gps_position(gps,0); // Recup latitude et longitude GPS pour actualiser depart
  get_gps_position(gps,1); // Recup latitude et longitude GPS pour actualiser position actuelle  

  // Calcul le cap demande
  theta=Recup_cap_sinus_180(theoricalheading); // Cap demande
  // Calcul des coordonnées du pts G
  float latGlongG[2]; // Coordonnées pts gauche de la route
  // Pts G
  theta=theta-(pi/4); 
  if (theta < -pi){ // Passage -pi/pi
    theta=theta+2*pi;
  }
  distance_1_pts_1_cap(radians(WP_lat[0]),radians(WP_long[0]), theta, AD_AG, latGlongG);

  // Calcul vitesse pour cap -45 deg
  Vitesse=0;
  Serial.println("Calcul vitesse pour cap -45"); // TEST
  for (int i=0;i<nbrVitesse;i++){ // Mise a 0
    Vitesses[i]=0;
  }
  Correction_Route(latGlongG[0],latGlongG[1]); // Corrige cap -45 deg 
  for (int i=0;i<nbrVitesse;i++){
    Correction_Route(latGlongG[0],latGlongG[1]); // Corrige cap -45 deg 
    Vitesses[i]=gps.f_speed_kmph(); // Fonction recup vitesse
    if (Vitesses[i] > 15){
      Vitesses[i]=0;
    }
    Serial.println(Vitesses[i]);
    delay(2000);
  }
  for (int i=0;i<nbrVitesse;i++){
    Vitesse=Vitesse+Vitesses[i];
  }
  Vitesse=Vitesse/nbrVitesse; // Vitesse2 moy 
  Vitesse=Vitesse*cos(pi/4); // VMG
  Serial.println("Vitesse -45"); // TEST
  Serial.println(Vitesse); // TEST
  delay(1000); // TEST

  if (Vitesse > 1){ // Si on avance, on garde cette route
    Serial.println("Route sur cap -45");
    delay(1000); // TEST
    WP_lat[2]=degrees(latGlongG[0]);
    WP_long[2]=degrees(latGlongG[1]);
    cap_optimise=true;
    return true;
  }
  return false;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void loop(){
  status_GPS(); // Attendre GPS connect
  if (depart_GPS==true){
    // Verif WP et met à jour
    coordonnees_route(); 

    // Corrige sens à la route
    Correction_Route(radians(WP_lat[2]),radians(WP_long[2])); 

    // Optimisation cap/vitesse
    if (virement_de_bord==false && depart==true && depart_optimisation==false){ // Si dans le sens de la route et dans la route et pas encore choisi meilleur cap/vitesse
      Serial.println("Optimisation_cap_vitesse"); // TEST
      depart_optimisation=Optimisation_cap_vitesse(); // false : erreur vitesse, true : ok
    }

    virement_de_bord=false;
  }
}










































